import pandas as pd
import numpy as np

from paper.trace.tools_coord_trans import xy_to_latilong


# 打印 col 基础信息
def print_col_first_max_min_mean(col, col_name):
    print(f"\n{col_name}:")
    print(f"  第一个: {col[0]}")
    print(f"  最大值: {col.max()}")
    print(f"  最小值: {col.min()}")
    print(f"  均值: {col.mean()}\n")


# 读取并转置 csv
def read_csv_transpose(path_csv):
    # 1. 读取 CSV 文件
    df = pd.read_csv(path_csv, header=None)
    # 2. 转置 DataFrame
    df_transposed = df.T
    return df_transposed


################################################ 主函数 #############################################
def format_matlab_caekf(path_csv_dataset, path_csv_xy, path_csv_coord):
    # 1. 读取 dataset 的经纬度数据
    df = pd.read_csv(path_csv_dataset)
    latitudes_ori, longitudes_ori = df['lati_smooth'].values, df['long_smooth'].values
    latitudes_ori, longitudes_ori = latitudes_ori[1:], longitudes_ori[1:]
    lati_0, long_0 = latitudes_ori[0], longitudes_ori[0]
    # 2. 读取 CAEKF 算法结果 .csv 并获取 x 和 y
    df_transposed = read_csv_transpose(path_csv_xy)
    caekf_x = df_transposed.iloc[:, 0]    # 第一列: x
    caekf_y = df_transposed.iloc[:, 1]    # 第二列: y
    print_col_first_max_min_mean(caekf_x, "caekf_x")
    print_col_first_max_min_mean(caekf_y, "caekf_y")
    # 3. 将 x 和 y 转化为经纬度
    latitudes, longitudes = xy_to_latilong(lati_0, long_0, caekf_x, caekf_y)
    print_col_first_max_min_mean(longitudes, "longitudes")
    print_col_first_max_min_mean(latitudes, "latitudes")
    # 打印结果示例
    print("转换后的经纬度坐标:")
    for i in range(min(5, len(longitudes))):  # 只打印前5个作为示例
        print(f"Point {i}: Longitude = {longitudes[i]:.8f}, Latitude = {latitudes[i]:.8f}")
    print("----------------------------------------------------------------")
    # 4. 保存到 CSV 文件
    result_df = pd.DataFrame({
        'Longitude_ori': longitudes_ori,
        'Latitude_ori': latitudes_ori,
        'Longitude': longitudes,
        'Latitude': latitudes,
        'caekf_x': caekf_x,
        'caekf_y': caekf_y
    })
    result_df.to_csv(path_csv_coord, index=False)
    print(f"\n结果已保存到 {path_csv_coord}")


def format_matlab_immukf(path_csv_dataset, path_csv_xy, path_csv_coord):
    # 1. 读取 dataset 的经纬度数据
    df = pd.read_csv(path_csv_dataset)
    latitudes_ori, longitudes_ori = df['lati_smooth'].values, df['long_smooth'].values
    latitudes_ori, longitudes_ori = latitudes_ori[1:], longitudes_ori[1:]
    lati_0, long_0 = latitudes_ori[0], longitudes_ori[0]
    # 2. 读取 csv 并获取 x 和 y
    df_transposed = read_csv_transpose(path_csv_xy)
    raw_col_x_str = df_transposed.iloc[:, 0]    # 第一列: x
    raw_col_y_str = df_transposed.iloc[:, 1]    # 第二列: y
    # 使用 pandas 的 apply 方法对每行进行处理
    immukf_x = raw_col_x_str.apply(lambda x: complex(x.replace('i', 'j'))).apply(lambda c: c.real)
    # 处理 y 列：同样转换为复数，然后提取实部
    immukf_y = raw_col_y_str.apply(lambda x: complex(x.replace('i', 'j'))).apply(lambda c: c.real)
    # 打印数据
    print_col_first_max_min_mean(immukf_x, "immukf_x")
    print_col_first_max_min_mean(immukf_y, "immukf_y")
    # 3. 将 x 和 y 转化为经纬度
    latitudes, longitudes = xy_to_latilong(lati_0, long_0, immukf_x, immukf_y)
    print_col_first_max_min_mean(longitudes, "longitudes")
    print_col_first_max_min_mean(latitudes, "latitudes")
    # 打印结果示例
    print("转换后的经纬度坐标:")
    for i in range(min(5, len(longitudes))):  # 只打印前5个作为示例
        print(f"Point {i}: Longitude = {longitudes[i]:.8f}, Latitude = {latitudes[i]:.8f}")
    print("----------------------------------------------------------------")
    # 3. 保存到 CSV 文件
    result_df = pd.DataFrame({
        'Longitude_ori': longitudes_ori,
        'Latitude_ori': latitudes_ori,
        'Longitude': longitudes,
        'Latitude': latitudes,
        'immukf_x': immukf_x,
        'immukf_y': immukf_y
    })
    result_df.to_csv(path_csv_coord, index=False)
    print(f"\n结果已保存到 {path_csv_coord}")



if __name__=="__main__":
    format_matlab_caekf('uav_dataset_interpl_test_2025-10-21.csv',
                        'caekf_out_Xe_UAV_Measure_1_test_2025-10-21.csv',
                        'caekf_out_Xe_UAV_Measure_1_test_coord_2025-10-21.csv')
    format_matlab_immukf('uav_dataset_interpl_test_2025-10-21.csv',
                         'immukf_out_Xe_UAV_Measure_1_test_2025-10-21.csv',
                         'immukf_out_Xe_UAV_Measure_1_test_coord_2025-10-21.csv')